#include "task.h"

static unsigned int nextTaskID = 0;
static unsigned int getTaskID (void)
{
	nextTaskID ++;
	return nextTaskID;
}

void taskDelay (unsigned int ticks)
{
	struct task *task;
	task = getRunningTask ();
	task->wait = (ticks + getTickCount ()) & ~(1 << 31);
	forceContextSwitch ();
}

static struct task tasks [NUMBER_TASKS];
unsigned int taskCreate (void (*function) (void *parameters), unsigned int stackWords, void *parameters)
{
	unsigned int taskID;
	struct task *task;
	void *stack;

	taskID = getTaskID ();
	task = &tasks [taskID];
	stack = allocateStack (stackWords);

	task->id = taskID;
	task->epc = (void *) function;
	task->gpr [29] = (unsigned int) stack;
	task->gpr [4] = (unsigned int) parameters;
	task->wait = 0;

	RQenqueue (task);

	return taskID;
}

void createIdleTask (void)
{
	struct task *idle;
	void *stack;

	idle = &tasks [0];
	stack = allocateStack (64);
	idle->id = 0;
	idle->epc = (void *) idleTask;
	idle->gpr [29] = (unsigned int) stack;
	idle->wait = 0;
	RQenqueue (idle);
}

void startScheduler (void)
{
	unsigned int pbclock;
	createIdleTask ();
	pbclock = SYSTEMConfigPerformance (80000000);

	OpenTimer1 (T1_ON | T1_IDLE_CON | T1_PS_1_64, pbclock / 64000); // Runs every millisecond
	ConfigIntTimer1 (T1_INT_ON | T1_INT_PRIOR_6);

	mConfigIntCoreSW0 (CSW_INT_ON | CSW_INT_PRIOR_1 | CSW_INT_SUB_PRIOR_0);

	INTEnableSystemMultiVectoredInt();


	forceContextSwitch ();
	while (1);

}
#if 0
void idleTask (void)
{
	while (1)
	{
		;
	}
}
#endif
